/* Includes ------------------------------------------------------------------------------------------------*/
#include "ht32.h"
#include "string.h"
#include "crc16.h"
#include "iap_handler.h"
#include "iap_uart.h"
#ifdef UART_PRO_FSC
#include "hal_uart.h"
#include "cmd_fsc.h"
#endif
/* Private typedef -----------------------------------------------------------------------------------------*/
#define LOADER_VERSION          (102)
typedef struct {
  vu32 MDID;                             /* offset  0x180 Flash Manufacturer and Device ID Register (MDID)  */
  vu32 PNSR;                             /* offset  0x184 Flash Page Number Status Register (PNSR)          */
  vu32 PSSR;                             /* offset  0x188 Flash Page Size Status Register (PSSR)            */
  vu32 DID;
} FMCInfo_TypeDef;

typedef u32 (*pFunction)();


/*  User Command count                                                                                      */
#define HEADER_LEN              (5)
#define MAX_RETURN_PAYLOAD      (64)
#define CRC_LEN                 (2)
#define MAX_RETURN_LEN          (HEADER_LEN + MAX_RETURN_PAYLOAD + CRC_LEN) // Header + MaxPayload + crc

#define MAX_CMD_LEN             (HEADER_LEN + 4 + 4 + CMD_PAYLOAD_LEN + CRC_LEN)
#define MAX_TOKENS              (3)
#define IAP_CMD_COUNT           (10)
#define CMD_COUNT               (IAP_CMD_COUNT)
#define INF_COUNT               (5)

#define USER_CMD_START          (0x50)
#define CMD_PAYLOAD_ADDR        (HEADER_LEN + 4 + 4)
#define CMD_PAYLOAD_LEN         (64)

#define PREFIX                  (0x55)

#define FMCINFO_BASE            (0x40080180)
#define FMCINFO                 ((FMCInfo_TypeDef*) FMCINFO_BASE)

#if defined(LIBCFG_FLASH_2PAGE_PER_WPBIT)
  #define PPBIT_PRE_PAGE         2
#else
  #define PPBIT_PRE_PAGE         1
#endif

#define LOADER_MODE             (0x1)
#define LOADER_FLASH_START      (IAP_CODE_SIZE + IAP_APINFO_SIZE)
#define LOADER_CHIP_NAME        (0)
#define LOADER_PAGE_SIZE        (FMCINFO->PSSR)
#define LOADER_PPBIT_NUM        ((FMCINFO->PNSR) - (IAP_CODE_SIZE/LIBCFG_FLASH_PAGESIZE/PPBIT_PRE_PAGE) - 1)
#define LOADER_FLASH_NUM        ((LIBCFG_FLASH_SIZE - IAP_CODE_SIZE - IAP_VERSION_SIZE)/LIBCFG_FLASH_PAGESIZE)
#define LOADER_FULL_CHIP_NAME   ((FMCINFO->DID) & 0x0FFFFFFF)

#define LOADER_INFO0            (u32)((LOADER_MODE      << 28) | (LOADER_VERSION     << 16) | LOADER_CHIP_NAME)
#define LOADER_INFO1            (u32)((LOADER_PAGE_SIZE << 16) | (LOADER_FLASH_START << 0))
#define LOADER_INFO2            (u32)((LOADER_FLASH_NUM << 16) | (LOADER_PPBIT_NUM   << 0))
#define LOADER_INFO3            (u32)(IAP_CODE_SIZE)
#define LOADER_INFO4            (u32)(LOADER_FULL_CHIP_NAME)

#define FLASH_CMD_PROGRAM       ((u32)0x00000004)
#define FLASH_SEND_MAIN         ((u32)0x00000014)

/* Private function prototypes -----------------------------------------------------------------------------*/
u32 _IAP_CMD0(void);
u32 _IAP_Erase(u32 type, u32 saddr, u32 eaddr);
u32 _IAP_Flash(u32 type, u32 saddr, u32 eaddr);
u32 _IAP_CRC(u32 crc, u32 saddr, u32 length);
u32 _IAP_Info(void);
void _IAP_Exit(void);
u32 _IAP_Reset(u32 uMode);
u32 _IAP_GetBootMode(void);

static void FLASH_Operation(u32 type, u32 PageAddress, u32 Data);

/* Private macro -------------------------------------------------------------------------------------------*/
#define WriteByte(data)         IAP_UART_Send((u8*)&data, 1)
#define WriteBytes(pData, len)  IAP_UART_Send(pData, len)
#define ReadByte(pData)         IAP_UART_Get(pData, 1, 100)

/* Private variables ---------------------------------------------------------------------------------------*/
static u32 gu32Infotable[INF_COUNT];

__ALIGN4 static u8 gu8CmdBuffer[MAX_CMD_LEN];
__ALIGN4 static u8 gu8ReturnBuffer[MAX_RETURN_LEN];
static u32 u32BufferIndex;
static u32 u32ReturnBufferIndex;
static vu16 gRxCmdTimeoutCnt = 0;
static vu16 gHeartbeatTimeoutCnt = 0;

u32 sp_backup,sp_address_backup;


static u8 CalculateHeaderCheckSum(u8* pCmdBuffer);
static void AdjustReturnPackage(u8* pBuf, u8 length);
static void ParseCmd(void);

static const pFunction pFComHandlerTable[CMD_COUNT] =
{
  (pFunction)_IAP_Erase,
  (pFunction)_IAP_Flash,
  (pFunction)_IAP_CRC,
  (pFunction)_IAP_Info,
  (pFunction)_IAP_Reset,
  (pFunction)_IAP_Exit,
  (pFunction)_IAP_CMD0,
  (pFunction)_IAP_CMD0,
  (pFunction)_IAP_GetBootMode,
  (pFunction)_IAP_CMD0,
};


void CKCU_Configuration(void)
{
    CKCU_PeripClockConfig_TypeDef CKCUClock = {{ 0 }};
    CKCUClock.Bit.PDMA       = 1;
    CKCUClock.Bit.PA         = 1;
    CKCUClock.Bit.PB         = 1;
    CKCUClock.Bit.PC         = 1;
    CKCUClock.Bit.PF         = 1;
    CKCUClock.Bit.USART0     = 0;
    CKCUClock.Bit.UART0      = 1;
    CKCUClock.Bit.SPI0       = 1;
    CKCUClock.Bit.SPI1       = 0;
    CKCUClock.Bit.AFIO       = 1;
    CKCUClock.Bit.GPTM0      = 0;
    CKCUClock.Bit.PWM0       = 1;
    CKCUClock.Bit.PWM1       = 1;
    CKCUClock.Bit.BFTM0      = 1;
    CKCUClock.Bit.BFTM1      = 0;
    CKCUClock.Bit.BKP        = 1;
    CKCU_PeripClockConfig(CKCUClock, ENABLE);
		
    while(PWRCU_CheckReadyAccessed() != PWRCU_OK);

    NVIC_EnableIRQ(LVD_BOD_IRQn);	

    PWRCU_SetLVDS(PWRCU_LVDS_3V15);
    PWRCU_LVDCmd(ENABLE);
}
#define SPI_CS_Pin         GPIO_PIN_3
#define SPI_CS_GPIO_Port   HT_GPIOA

#define RST_7006_Pin			 GPIO_PIN_8
#define RST_7006_GPIO_Port HT_GPIOB

#define SPI_DIR_Pin0 			 GPIO_PIN_7        //MPI Mode0
#define SPI_DIR_Pin1 			 GPIO_PIN_6        //MPI Mode1
#define SPI_DIR_GPIO_Port	 HT_GPIOC

#define MSPI_RESET_Pin  GPIO_PIN_12      //RST_SENS
#define MSPI_RESET_Port HT_GPIOB

#define LDO_CTRL_Pin  GPIO_PIN_6        //LDO_CTR
#define LDO_CTRL_Port HT_GPIOB

#define SEN_INT_Pin  GPIO_PIN_0        
#define SEN_INT_Port HT_GPIOF

#define TOUCH_CTRL_PIN  GPIO_PIN_7
#define TOUCH_CTRL_PORT HT_GPIOB

/** Configure pins as
        * Analog
        * Input
        * Output
        * EVENT_OUT
        * EXTI
        * Free pins are configured automatically as Analog (this feature is enabled through
        * the Code Generation settings)
*/
void MX_GPIO_Init(void)
{
		// PWM
		//AFIO_GPxConfig(GPIO_PA, AFIO_PIN_9, AFIO_FUN_PWM);
		//GPIO_DriveConfig(HT_GPIOA, GPIO_PIN_9, GPIO_DV_8MA);
		// CKOUT
		AFIO_GPxConfig(GPIO_PA, AFIO_PIN_9, AFIO_FUN_SYSTEM);
		GPIO_DriveConfig(HT_GPIOA, GPIO_PIN_9, GPIO_DV_8MA);

		// UART Tx Rx
		AFIO_GPxConfig(GPIO_PB, AFIO_PIN_2 | AFIO_PIN_3, AFIO_FUN_USART_UART);
	
		// Input 
		GPIO_SetOutBits(HT_GPIOA, GPIO_PIN_9);
		GPIO_DirectionConfig(HT_GPIOA, GPIO_PIN_9, GPIO_DIR_IN);
		// Output 
		GPIO_SetOutBits(SPI_CS_GPIO_Port, SPI_CS_Pin);
		GPIO_DirectionConfig(SPI_CS_GPIO_Port, SPI_CS_Pin, GPIO_DIR_OUT);

		GPIO_SetOutBits(RST_7006_GPIO_Port, RST_7006_Pin);
		GPIO_DirectionConfig(RST_7006_GPIO_Port, RST_7006_Pin, GPIO_DIR_OUT);

		GPIO_SetOutBits(SPI_DIR_GPIO_Port, SPI_DIR_Pin0|SPI_DIR_Pin1);
		GPIO_DirectionConfig(SPI_DIR_GPIO_Port, SPI_DIR_Pin0|SPI_DIR_Pin1, GPIO_DIR_OUT);

		AFIO_GPxConfig(GPIO_PB, AFIO_PIN_12, AFIO_FUN_GPIO);
		GPIO_SetOutBits(MSPI_RESET_Port, MSPI_RESET_Pin);
		GPIO_DirectionConfig(MSPI_RESET_Port, MSPI_RESET_Pin, GPIO_DIR_OUT);

		GPIO_SetOutBits(LDO_CTRL_Port, LDO_CTRL_Pin);
		GPIO_DirectionConfig(LDO_CTRL_Port, LDO_CTRL_Pin, GPIO_DIR_OUT);

		// Input 
		GPIO_PullResistorConfig(SEN_INT_Port, SEN_INT_Pin, GPIO_PR_UP);
		GPIO_InputConfig(SEN_INT_Port, SEN_INT_Pin, ENABLE);

        GPIO_ClearOutBits(TOUCH_CTRL_PORT, TOUCH_CTRL_PIN);
        GPIO_DirectionConfig(TOUCH_CTRL_PORT, TOUCH_CTRL_PIN, GPIO_DIR_OUT);

		// SPI Flash SCK, MOSI, CS 
		GPIO_DriveConfig(HT_GPIOA, GPIO_PIN_0 | GPIO_PIN_1 | GPIO_PIN_3, GPIO_DV_8MA);

		// SPI Flash SCK, MOSI, MISO
		AFIO_GPxConfig(GPIO_PA, AFIO_PIN_0 | AFIO_PIN_1 | AFIO_PIN_2, AFIO_FUN_SPI);
}

void CKOUTConfig(void)
{
		CKCU_PeripClockConfig_TypeDef CKCUClock = {{ 0 }};
		CKCU_CKOUTInitTypeDef CKOUTInit;
		
		CKOUTInit.CKOUTSRC = CKCU_CKOUTSRC_REFCK;
		CKCU_CKOUTConfig(&CKOUTInit);
		
		CKCU_SetCKREFPrescaler(CKCU_CKREFPRE_DIV6);
		CKCUClock.Bit.CKREF=1;
		CKCU_PeripClockConfig(CKCUClock,ENABLE);
}
#ifndef UART_PRO_FSC
#define HT_UARTx    HT_UART0
#define UARTx_IRQn  UART0_IRQn

static const int32_t BaudRate_Buf[] =
{
    9600,115200,14400,19200,38400,57600,460800,921600
};

/* USART1 init function */
int32_t USART_Config(uint8_t Mode,	uint32_t BaudRate)
{
    uint32_t i;
    uint32_t len = sizeof(BaudRate_Buf)/sizeof(int32_t);

    for(i=0;i<len;i++)
    {
        if(BaudRate == BaudRate_Buf[i])
        {
            break;
        }
    }

    if(i==len)
        return -1;

	USART_InitTypeDef USART_InitStructure;

  USART_InitStructure.USART_BaudRate = BaudRate;
  USART_InitStructure.USART_WordLength = USART_WORDLENGTH_8B;
  USART_InitStructure.USART_StopBits = USART_STOPBITS_1;
  USART_InitStructure.USART_Parity = USART_PARITY_NO;
  USART_InitStructure.USART_Mode = USART_MODE_NORMAL;
  USART_Init(HT_UARTx, &USART_InitStructure);
  USART_TxCmd(HT_UARTx, ENABLE);
  USART_RxCmd(HT_UARTx, ENABLE);


  if(Mode == 0)
  {
		/* Seting COM1_PORT interrupt-flag                                                                        */
		USART_IntConfig(HT_UARTx, USART_INT_RXDR, ENABLE);
		USART_TXTLConfig(HT_UARTx, USART_TXTL_00);
		//USART_RXTLConfig(RETARGET_USART_PORT, USART_RXTL_01);
		//USART_IntConfig(RETARGET_USART_PORT, USART_INT_RXDR, ENABLE);
		NVIC_EnableIRQ(UARTx_IRQn);
  }
  return 0;
}
#endif
/*********************************************************************************************************//**
  * @brief  IAP mode initialization.
  * @retval None
  ***********************************************************************************************************/
void IAP_Init(void)
{
  gu32Infotable[0] = LOADER_INFO0;
  gu32Infotable[1] = LOADER_INFO1;
  gu32Infotable[2] = LOADER_INFO2;
  gu32Infotable[3] = LOADER_INFO3;
  gu32Infotable[4] = LOADER_INFO4;

  //iap_uart_Configuration();
	CKCU_Configuration();
	MX_GPIO_Init();
	CKOUTConfig();
	iap_uart_Configuration();
#ifdef UART_PRO_FSC
	UART_Init(115200, FSC_DataHandler);
#else
	USART_Config(0, 115200);
#endif
}


/*********************************************************************************************************//**
  ***********************************************************************************************************/
void IAP_Handler(void)
{ 
  static u8 gRxCmdState = 0;
  u8 c;

  while(ReadByte(&c))
  {
    switch (gRxCmdState)
    {
      case 0:
      {
        if(c == 0x55)
        {
          u32BufferIndex = 0;
          gu8CmdBuffer[u32BufferIndex++] = c;
          gRxCmdState++;
          gRxCmdTimeoutCnt = 10;
          continue;
        }
        return;
      }
      case 1:
      {
        gu8CmdBuffer[u32BufferIndex++] = c;
        gRxCmdState++;
        gRxCmdTimeoutCnt = 10;
        continue;
      }
      case 2:
      {
        gRxCmdTimeoutCnt = 500;

        gu8CmdBuffer[u32BufferIndex++] = c;
        
        if(u32BufferIndex >= gu8CmdBuffer[1])
        {
          ParseCmd();
          gRxCmdState = 0;
          gHeartbeatTimeoutCnt = 100;
          return;
        }
        
        continue;
      }
      default:
      {
        gRxCmdState = 0;

        break;
      }
    }
  }        
  
  if(gRxCmdTimeoutCnt == 0)
  {
    gRxCmdState = 0;
  }
}

/*********************************************************************************************************//**
  * @brief  Reset Command.
  * @param  uMode: Mode after reset
  * @retval FALSE or TRUE
  ***********************************************************************************************************/
u32 _IAP_Reset(u32 uMode)
{
  if (uMode == 0)
  {
    ww(BOOT_MODE_ID_ADDR, BOOT_MODE_AP);
  }
  else
  {
    ww(BOOT_MODE_ID_ADDR, BOOT_MODE_IAP);
  }
#ifndef UART_PRO_FSC
  gu8ReturnBuffer[2] = CMD_SUCCESS;
  AdjustReturnPackage(gu8ReturnBuffer, 5);
  WriteBytes(gu8ReturnBuffer, gu8ReturnBuffer[1]);
#endif
  NVIC_SystemReset();

  return TRUE;
}

/*********************************************************************************************************//**
  * @brief  Check AP is valid or not.
  * @retval FALSE or TRUE
  ***********************************************************************************************************/
u32 IAP_isAPValid(void)
{
  vu32 SP, PC;

  /* Check Stack Point in range                                                                             */
  SP = rw(IAP_APFLASH_START);
  if (SP < IAP_APSRAM_START || SP > IAP_SRAM_END)
  {
    return FALSE;
  }

  /* Check PC in range                                                                                      */
  PC = rw(IAP_APFLASH_START + 0x4);
  if (PC < IAP_APFLASH_START || PC > IAP_APFLASH_END)
  {
    return FALSE;
  }
  
  return TRUE;
}


#if defined (__CC_ARM)
/*********************************************************************************************************//**
  * @brief  Jump to user application by change PC.
  * @param  address: Start address of user application
  * @retval None
  ***********************************************************************************************************/
__asm void IAP_GoCMD(u32 address)
{
  LDR R1, [R0]
  MOV SP, R1
  LDR R1, [R0, #4]
  BX R1
}
#elif defined (__ICCARM__)
void IAP_GoCMD(u32 address)
{
  __asm("LDR R1, [R0]");
  __asm("MOV SP, R1");
  __asm("LDR R1, [R0, #4]");
  __asm("BX R1");
}
#endif

/*********************************************************************************************************//**
  * @brief  TBD
  * @retval None
  ***********************************************************************************************************/
u8 CalculateHeaderCheckSum(u8* pCmdBuffer)
{
  u8 i;
  u8 sum = 0;
  
  for(i=0 ; i < 4 ; i++)
  {
    sum += pCmdBuffer[i];
  }
  sum = (~sum ) + 1;
  return sum;
}

/*********************************************************************************************************//**
  * @brief  TBD
  * @retval None
  ***********************************************************************************************************/
void AdjustReturnPackage(u8* pBuf, u8 length)
{
  u16 crc;
  
  pBuf[0] = 0x55;
  pBuf[1] = length + 2; // Add CRC length
  pBuf[4] = CalculateHeaderCheckSum(pBuf);
  
  crc = CRC16(0, (u8 *)(&pBuf[0]), length);
  memcpy((u8*)&pBuf[length], (u8*)&crc, 2);
}

/*********************************************************************************************************//**
  * @brief  TBD
  * @retval None
  ***********************************************************************************************************/
static void ParseCmd(void)
{
  u32 u32Parameter[MAX_TOKENS];
  u16 crc;
  u16 crcValue;
  u8 len;
  
  u32ReturnBufferIndex = 5;
  gu8ReturnBuffer[3] = 0x0;
  
  /*------------------------------------------------------------------------------------------------------*/
  /* Check CRC value of command packet                                                                    */
  /*------------------------------------------------------------------------------------------------------*/
  len = gu8CmdBuffer[1];
  crc = gu8CmdBuffer[len-2] | ((u16)gu8CmdBuffer[len-1] << 8);
  crcValue = CRC16(0, (u8 *)(&gu8CmdBuffer[0]), len-2);

  if (gu8CmdBuffer[2] >= USER_CMD_START)
  {
    gu8CmdBuffer[2] = gu8CmdBuffer[2] - USER_CMD_START + IAP_CMD_COUNT;
  }
  
  /*------------------------------------------------------------------------------------------------------*/
  /* Check command is valid and CRC is correct                                                            */
  /*------------------------------------------------------------------------------------------------------*/
  if (gu8CmdBuffer[2] > CMD_COUNT)
  {
    /*----------------------------------------------------------------------------------------------------*/
    /* Command invalid or CRC error. Return 'F' and clear command buffer                                  */
    /*----------------------------------------------------------------------------------------------------*/
    gu8ReturnBuffer[2] = CMD_FAILED;
    gu8ReturnBuffer[3] = 0x1;
  }
  else if (crc != crcValue)
  {
    gu8ReturnBuffer[2] = CMD_FAILED;
    gu8ReturnBuffer[3] = 0x1;
  }
  else
  {
    /*----------------------------------------------------------------------------------------------------*/
    /* Prepare parameter and execution command                                                            */
    /*----------------------------------------------------------------------------------------------------*/
    u32Parameter[0] = gu8CmdBuffer[3];
    memcpy((u8*)&u32Parameter[1], (u8*)&gu8CmdBuffer[5], 4);
    memcpy((u8*)&u32Parameter[2], (u8*)&gu8CmdBuffer[9], 4);
    
    gu8ReturnBuffer[2] = (*pFComHandlerTable[gu8CmdBuffer[2]])(u32Parameter[0], u32Parameter[1], u32Parameter[2]);
  }

  /*------------------------------------------------------------------------------------------------------*/
  /* Send Result to Host                                                                                  */
  /*------------------------------------------------------------------------------------------------------*/  
  AdjustReturnPackage(gu8ReturnBuffer, u32ReturnBufferIndex);
  WriteBytes(gu8ReturnBuffer, gu8ReturnBuffer[1]);
}

/*********************************************************************************************************//**
  * @brief  
  * @retval 
  ***********************************************************************************************************/
static u32 _IAP_CMD0(void)
{
  return CMD_SUCCESS;
}

/*********************************************************************************************************//**
  * @brief  
  * @retval 
  ***********************************************************************************************************/
static u32 _IAP_GetBootMode(void)
{
  gu8ReturnBuffer[u32ReturnBufferIndex++] = 0xF5; // IAP mode
  
  return CMD_SUCCESS;
}

/*********************************************************************************************************//**
  * @brief  Send information to Host.
  * @retval Always success (CMD_SUCCESS)
  ***********************************************************************************************************/
static u32 _IAP_Info(void)
{
//  printf("INFO\r\n");
  gu8ReturnBuffer[2] = CMD_SUCCESS;

  memcpy((u8*)&gu8ReturnBuffer[5], (u8*)&gu32Infotable[0], 12);
  AdjustReturnPackage(gu8ReturnBuffer, 5+12);
  WriteBytes(gu8ReturnBuffer, gu8ReturnBuffer[1]);

  memcpy((u8*)&gu8ReturnBuffer[u32ReturnBufferIndex], (u8*)&gu32Infotable[3], 8);
  u32ReturnBufferIndex += 8;
  
  return CMD_SUCCESS;
}

/*********************************************************************************************************//**
  * @brief  Download image for program or verify.
  * @param  type: Program or verify
  *         @arg CMD_PROGRAM: Program mode
  *         @arg CMD_VERIFY: Verify mode
  *         @arg CMD_BLANK: Blank check mode
  *         @arg CMD_READ: Read mode
  * @param  saddr: Start address
  * @param  eaddr: End address
  * @param  buffer: point of data buffer
  * @retval CMD_SUCCESS or CMD_FAILED
  ***********************************************************************************************************/
u32 _IAP_Flash(u32 type, u32 saddr, u32 eaddr)
{
  u32 i, data;
  u8 *buffer = (u8 *)(&gu8CmdBuffer[13]);
  FLASH_OptionByte Option;

//  printf("Flash:%x %x %x\r\n",type,saddr,eaddr);
  /*--------------------------------------------------------------------------------------------------------*/
  /* When Security protection is enabled, read operation is not allowed                                     */
  /*--------------------------------------------------------------------------------------------------------*/
  FLASH_GetOptionByteStatus(&Option);

  if (type == CMD_PROGRAM)
  {
    /* Blank Check before programming                                                                       */
    for (i = 0; i < (eaddr-saddr + 1); i += 4)
    {
      if (rw(saddr + i) != 0xFFFFFFFF )
      {
        gu8ReturnBuffer[3] = 0x4;
        return CMD_FAILED;
      }
    }
  }

  /*--------------------------------------------------------------------------------------------------------*/
  /* Program                                                                                                */
  /*--------------------------------------------------------------------------------------------------------*/
  if (type == CMD_PROGRAM)
  {
    if(saddr == sp_address_backup)
    {
      memcpy((u8*)&sp_backup, (u8*)buffer, 4);
//      printf("save sp:%x\r\n",sp_backup);
      saddr += 4;
      buffer += 4;
    }

    while (saddr <= eaddr)
    {
      u32 tmp;
      memcpy((u8*)&tmp, (u8*)buffer, 4);

      FLASH_Operation(FLASH_CMD_PROGRAM, saddr, tmp);
      saddr += 4;
      buffer += 4;
    }
  }

  while (saddr <= eaddr)
  {
    if (saddr > 0x1FF003FC)
    {
      data = 0;
    }
    else
    {
      data = rw(saddr);
    }
    /*------------------------------------------------------------------------------------------------------*/
    /* Verify                                                                                               */
    /*------------------------------------------------------------------------------------------------------*/
    if (type == CMD_VERIFY)
    {
      u32 tmp;
      memcpy((u8*)&tmp, (u8*)buffer, 4);

      if (data != tmp)
      {
        gu8ReturnBuffer[3] = 0x4;
        return CMD_FAILED;
      }
    }
    /*------------------------------------------------------------------------------------------------------*/
    /* Blank                                                                                                */
    /*------------------------------------------------------------------------------------------------------*/
    else if (type == CMD_BLANK)
    {
      if (data != 0xFFFFFFFF)
      {
        gu8ReturnBuffer[3] = 0x4;
        return CMD_FAILED;
      }
    }
    /*--------------------------------------------------------------------------------------------------------*/
    /* Read                                                                                                   */
    /*--------------------------------------------------------------------------------------------------------*/   
    else
    {
      if (Option.MainSecurity == 1)
      {
        if ((saddr >= IAP_CODE_SIZE + 0x800) && (saddr < (IAP_CODE_SIZE + 0x800 + 48)) )
        {
          memcpy((u8*)&gu8ReturnBuffer[u32ReturnBufferIndex], (u8*)&data, 4);
          u32ReturnBufferIndex += 4;
        }

        else
        {
          data = 0;
          memcpy((u8*)&gu8ReturnBuffer[u32ReturnBufferIndex], (u8*)&data, 4);
          u32ReturnBufferIndex += 4;
        }
      } 
      else
      {     
        memcpy((u8*)&gu8ReturnBuffer[u32ReturnBufferIndex], (u8*)&data, 4);
        u32ReturnBufferIndex += 4;
      }
    }    

    saddr += 4;
    buffer += 4;
  }

  return CMD_SUCCESS;
}

u32 IAP_Flash(u32 type, u32 saddr, u32 eaddr, u8 *buffer)
{
  u32 i, data;
  FLASH_OptionByte Option;

//  printf("Flash:%x %x %x\r\n",type,saddr,eaddr);
  /*--------------------------------------------------------------------------------------------------------*/
  /* When Security protection is enabled, read operation is not allowed                                     */
  /*--------------------------------------------------------------------------------------------------------*/
  FLASH_GetOptionByteStatus(&Option);

  if (type == CMD_PROGRAM)
  {
    /* Blank Check before programming                                                                       */
    for (i = 0; i < (eaddr-saddr + 1); i += 4)
    {
      if (rw(saddr + i) != 0xFFFFFFFF )
      {
        gu8ReturnBuffer[3] = 0x4;
        return CMD_FAILED;
      }
    }
  }

  /*--------------------------------------------------------------------------------------------------------*/
  /* Program                                                                                                */
  /*--------------------------------------------------------------------------------------------------------*/
  if (type == CMD_PROGRAM)
  {
    if(saddr == sp_address_backup)
    {
      memcpy((u8*)&sp_backup, (u8*)buffer, 4);
//      printf("save sp:%x\r\n",sp_backup);
      saddr += 4;
      buffer += 4;
    }

    while (saddr < eaddr)
    {
      u32 tmp;
      memcpy((u8*)&tmp, (u8*)buffer, 4);

      FLASH_Operation(FLASH_CMD_PROGRAM, saddr, tmp);
      saddr += 4;
      buffer += 4;
    }
  }

  while (saddr < eaddr)
  {
    if (saddr > 0x1FF003FC)
    {
      data = 0;
    }
    else
    {
      data = rw(saddr);
    }
    /*------------------------------------------------------------------------------------------------------*/
    /* Verify                                                                                               */
    /*------------------------------------------------------------------------------------------------------*/
    if (type == CMD_VERIFY)
    {
      u32 tmp;
      memcpy((u8*)&tmp, (u8*)buffer, 4);

      if (data != tmp)
      {
        gu8ReturnBuffer[3] = 0x4;
        return CMD_FAILED;
      }
    }
    /*------------------------------------------------------------------------------------------------------*/
    /* Blank                                                                                                */
    /*------------------------------------------------------------------------------------------------------*/
    else if (type == CMD_BLANK)
    {
      if (data != 0xFFFFFFFF)
      {
        gu8ReturnBuffer[3] = 0x4;
        return CMD_FAILED;
      }
    }
    /*--------------------------------------------------------------------------------------------------------*/
    /* Read                                                                                                   */
    /*--------------------------------------------------------------------------------------------------------*/   
    else
    {
      if (Option.MainSecurity == 1)
      {
        if ((saddr >= IAP_CODE_SIZE + 0x800) && (saddr < (IAP_CODE_SIZE + 0x800 + 48)) )
        {
          memcpy((u8*)&gu8ReturnBuffer[u32ReturnBufferIndex], (u8*)&data, 4);
          u32ReturnBufferIndex += 4;
        }

        else
        {
          data = 0;
          memcpy((u8*)&gu8ReturnBuffer[u32ReturnBufferIndex], (u8*)&data, 4);
          u32ReturnBufferIndex += 4;
        }
      } 
      else
      {     
        memcpy((u8*)&gu8ReturnBuffer[u32ReturnBufferIndex], (u8*)&data, 4);
        u32ReturnBufferIndex += 4;
      }
    }    

    saddr += 4;
    buffer += 4;
  }

  return CMD_SUCCESS;
}
/*********************************************************************************************************//**
  * @brief  Mass/Page Erase.
  * @param  type: Erase type
  *         @arg IAP_MASS_ERASE: Mass Erase (Not support in IAP mode)
  *         @arg IAP_PAGE_ERASE: Page Erase
  * @param  saddr: Start address
  * @param  eaddr: End address
  * @retval CMD_SUCCESS or CMD_FAILED
  ***********************************************************************************************************/
u32 _IAP_Erase(u32 type, u32 saddr, u32 eaddr)
{
  u32 i, j;
  
  if ( (type == IAP_MASS_ERASE) 
    || (saddr < IAP_CODE_SIZE) 
    || ((eaddr-saddr) > (LIBCFG_FLASH_SIZE - IAP_CODE_SIZE)))
  {
    gu8ReturnBuffer[3] = 0x3;
    return CMD_FAILED;
  }

  for (i = saddr, j = 0; i <= eaddr; i += FLASH_PAGE_SIZE, j++)
  {
    FLASH_ErasePage(i);
  }
  sp_address_backup = saddr;
  
//  printf("Erase:%x %x %x %x\r\n",type,saddr,eaddr,sp_address_backup);
  return CMD_SUCCESS;
}


/*********************************************************************************************************//**
  * @brief  Calculate CRC value.
  * @param  crc: Iinitial value of CRC (Usually as 0)
  * @param  saddr: Start address
  * @param  length: Length for CRC calculation
  * @retval Always success (CMD_SUCCESS)
  ***********************************************************************************************************/
u32 _IAP_CRC(u32 crc, u32 saddr, u32 length)
{
  FLASH_Operation(FLASH_CMD_PROGRAM, sp_address_backup, sp_backup); //write sp

//  printf("CRC:%x %x %x\r\n",crc,saddr,length);
  crc = CRC16(crc, (u8 *)saddr, length);

  memcpy((u8*)&gu8ReturnBuffer[u32ReturnBufferIndex], (u8*)&crc, 2);
  u32ReturnBufferIndex += 2;
  
  return CMD_SUCCESS;
}
uint16_t FscCRC16(uint8_t *buffer, uint32_t length);
u16 IAP_CRC(u32 saddr, u32 length)
{
	FLASH_Operation(FLASH_CMD_PROGRAM, sp_address_backup, sp_backup); //write sp

	return FscCRC16((u8 *)saddr, length);
}
/*********************************************************************************************************//**
  * @brief  Exit Loader mode.
  * @retval None
  ***********************************************************************************************************/
static void _IAP_Exit(void)
{
//  printf("EXIT\r\n");
  gu8ReturnBuffer[2] = CMD_SUCCESS;
  AdjustReturnPackage(gu8ReturnBuffer, 5);
  WriteBytes(gu8ReturnBuffer, gu8ReturnBuffer[1]);
  
  while (1);
}


/*********************************************************************************************************//**
  * @brief  FLASH_Operation
  * @retval None
  ***********************************************************************************************************/
static void FLASH_Operation(u32 type, u32 PageAddress, u32 Data)
{
  HT_FLASH->OCMR = type;
  HT_FLASH->TADR = PageAddress;
  HT_FLASH->WRDR = Data;
  HT_FLASH->OPCR = FLASH_SEND_MAIN;
}
